package com.campos.collection;

import android.content.Context;
import android.hardware.Sensor;
import android.hardware.SensorEvent;
import android.hardware.SensorEventListener;
import android.hardware.SensorManager;

import java.text.SimpleDateFormat;
import java.util.ArrayList;
import java.util.Date;


import com.campos.common.*;

public class IMUSensor {

    public static class IMUSensorData{
        public float[] angles, R, gravity, geomagnetic, gyroscope;
        public Date date;
        public IMUSensorData(){
            angles = new float[3];//用来保存最终的结果 弧度角
            gravity = new float[3];//用来保存加速度传感器的值
            gyroscope = new float[3];//用来保存陀螺仪传感器的值
            R = new float[9];//
            geomagnetic = new float[3];//用来保存地磁传感器的值
            date = null;
        }
        public IMUSensorData copy(){
            IMUSensorData nsd = new IMUSensorData();
            nsd.date = this.date;
            nsd.angles = this.angles.clone();
            nsd.geomagnetic = this.geomagnetic.clone();
            nsd.gravity = this.gravity.clone();
            nsd.gyroscope = this.gyroscope.clone();
            nsd.R = this.R.clone();
            return nsd;
        }
    }
    IMUSensorData sensorData;
    ArrayList<IMUSensorData> sensorDataBuffer;

    public static IMUSensor instance = new IMUSensor();


    private SensorManager manager;
    private MySensorEventListener listener;
    private Sensor magneticSensor, accelerometerSensor, gyroscopeSensor;

    public static class IMUSensorInfoCallback{
        public void callback(IMUSensorData sd ){

        }
    }

    public IMUSensorInfoCallback _callback = null;

    private double _lastTimeStamp = 0;
    private class MySensorEventListener implements SensorEventListener {
        @Override
        public void onSensorChanged(SensorEvent event) {
            if (event.sensor.getType() == Sensor.TYPE_ACCELEROMETER) {
                sensorData.gravity = event.values;
                //getValue();
            }
            if (event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD) {
                sensorData.geomagnetic = event.values;
            }
            if (event.sensor.getType() == Sensor.TYPE_GYROSCOPE) {
                sensorData.gyroscope = event.values;
            }

            SensorManager.getRotationMatrix(sensorData.R, null, sensorData.gravity, sensorData.geomagnetic);
            float[] R = sensorData.R;
            float[] R1 = new float[9];
            {
                R1[0] = R[0];R1[1] = -R[2];R1[2] = R[1];
                R1[3] = R[3];R1[4] = -R[5];R1[5] = R[4];
                R1[6] = R[6];R1[7] = -R[8];R1[8] = R[7];
            }
            SensorManager.getOrientation(R1, sensorData.angles);
            sensorData.angles[0] = (float)Math.toDegrees(sensorData.angles[0]);
            sensorData.angles[1] = (float)Math.toDegrees(sensorData.angles[1]*-1);
            sensorData.angles[2] = (float)Math.toDegrees(sensorData.angles[2]);
            Date dt = new Date();
            Long timestamp = dt.getTime();
            sensorData.date = dt;
            IMUSensorData imud = sensorData.copy();
            ContextData.instance.imuDataList.add(imud);
            ContextData.instance.currentHeading = sensorData.angles[0];
            ContextData.instance.currentPitch = sensorData.angles[1];
            ContextData.instance.currentRoll = sensorData.angles[2];
//            if(timestamp - _lastTimeStamp >= 80){
//                sensorDataBuffer.add(imud);
//                writeToFile();
//                _lastTimeStamp = timestamp;
//            }
            if(_callback != null){
                _callback.callback(sensorData);
            }
        }
        @Override
        public void onAccuracyChanged(Sensor sensor, int accuracy) {

        }
    }

    public void writeToFile(){
        if(sensorDataBuffer.size() < 50 ) return;
        StringBuffer sb = new StringBuffer();
        SimpleDateFormat simpleDateFormat = new SimpleDateFormat("yyyy-MM-dd HH:mm:ss.SSS");

        for (int i=0;i<sensorDataBuffer.size();i++){
            IMUSensorData sd = sensorDataBuffer.get(i);
            String timestr = simpleDateFormat.format(sd.date);
            String line = String.format("%s,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
                    timestr,sd.date.getTime()/1000.0,
                    sd.angles[0],sd.angles[1],sd.angles[2],
                    sd.gyroscope[0],sd.gyroscope[1],sd.gyroscope[2],
                    sd.gravity[0],sd.gravity[1],sd.gravity[2],
                    sd.geomagnetic[0],sd.geomagnetic[1],sd.geomagnetic[2]
                    );
            sb.append(line);
        }
        String path = Config.instance().getCurrentProjPath_IMUInfo();
        Util.WriteTxtFile(sb.toString(),path);
        sensorDataBuffer.clear();

//        "Azimuth：" + azimuth + azimuthAngle + "\nPitch：" + pitch + "\nRoll：" + roll
//                +"\nax:"+sd.gravity[0]+"\nay:"+sd.gravity[1]+"\naz:"+sd.gravity[2]
//                +"\ngx:"+sd.gyroscope[0]+"\ngy:"+sd.gyroscope[1]+"\ngz:"+sd.gyroscope[2]
//                +"\ngeomagneticx:"+sd.geomagnetic[0]+"\ngeomagneticy:"+sd.geomagnetic[1]+"\ngeomagneticz:"+sd.geomagnetic[2]
    }

    public void start(Context c){
        if(manager == null){
            //获取SensorManager
            manager = (SensorManager) c.getSystemService(Context.SENSOR_SERVICE);
            listener = new MySensorEventListener();
            //获取Sensor
            magneticSensor = manager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD);
            accelerometerSensor = manager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER);
            gyroscopeSensor = manager.getDefaultSensor(Sensor.TYPE_GYROSCOPE);

            sensorData = new IMUSensorData();
            sensorDataBuffer = new ArrayList<IMUSensorData>();
        }


        manager.registerListener(listener, magneticSensor, SensorManager.SENSOR_DELAY_GAME);
        manager.registerListener(listener, accelerometerSensor, SensorManager.SENSOR_DELAY_GAME);
        manager.registerListener(listener, gyroscopeSensor, SensorManager.SENSOR_DELAY_GAME);
    }

    public void stop(){
        manager.unregisterListener(listener);
    }
}
